#include "Stereo.h"
//#include "CameraCalibration.h"
#include <iostream>

namespace Stereo
{

#pragma region mousedata

struct Onemousedata
{
	int cntr;
	int num_points;
	std::vector<CvPoint> p;
	Onemousedata()
	{
		cntr=0;
		p.resize(num_points,cvPoint(0,0));
	}
	void Resize(int nb)
	{
	num_points=nb;
	p.assign(num_points,cvPoint(0,0));
	}
};
int global_points;

Onemousedata omdata;


IplImage *imageposit;

#pragma endregion
#pragma region mousehandler
void WaitKey(int key)
{
	cvWaitKey(key);
}


void On_Mouse1(int event, int x, int y, int flags, void* param)
{
		
	switch(event)
	{
	case CV_EVENT_LBUTTONDOWN:
		{
			int cntr = omdata.cntr;
			char cntr_s[5];
			CvFont font;
			cvInitFont(&font,CV_FONT_HERSHEY_PLAIN|CV_FONT_ITALIC,1,1,0,1);

			CvPoint ptr = cvPoint(x,y);

			if (cntr <global_points)
			{	
				
				itoa(cntr,cntr_s,10);
				omdata.p[cntr] = ptr;
				
				cvCircle(imageposit,omdata.p[cntr],1,cvScalar(0,0,255),3);
				
				

				cvPutText(imageposit,cntr_s,omdata.p[cntr],&font,cvScalar(0,0,255));

				//omdata.p[cntr].x-=390;
				//omdata.p[cntr].y=290-omdata.p[cntr].y;
				printf("(%d,%d)",omdata.p[cntr].x,omdata.p[cntr].y);

				cvShowImage("Camera Image", imageposit);
			}
			
			omdata.cntr +=  1;
		}
		break;
	}



}

void MouseHandeler(char * windowname,int type)
{
	 
  cvNamedWindow(windowname, CV_WINDOW_AUTOSIZE);
  if(type==1) cvSetMouseCallback(windowname,On_Mouse1,0);
}

#pragma endregion
#pragma region MatrixOperation
/** *********************print matrix****************************************************
  **/
void PrintMatrix(CvMat *matrix,char *matrixname)
{
	float x;
	printf("--------------------------------------------------------------------\n");
	printf("\n%s is:\n\n",matrixname);
	for(int i=0 ; i < matrix->rows;i++)
		
	{
			for(int j = 0;j < matrix->cols;j++)
			{
                x = cvmGet(matrix,i,j);
				printf("%f\t",x);
			}
			printf("\n");
	}
	printf("--------------------------------------------------------------------\n");


}
/** *********************set matrix[i,j]=value****************************************************
  **/
void SetValue(CvMat *A,int i,int j,float value)
{
	cvSetReal2D(A,i,j,value); 

}
/** *********************Show Image***************************************************
  **/
void ShowImage(char *windowname,IplImage *image)
{
	cvShowImage(windowname, image);
}
/** *********************return A*B****************************************************
  **/
CvMat* MultiplyMatrix(const CvMat *A,const CvMat *B)
{	
	CvMat *res=cvCreateMat(A->height,B->width,CV_64F);
	cvMatMulAdd(A,B,0,res);
	return res;
}
/** *********************return A*B*C****************************************************
  **/
CvMat* MultiplyMatrix(const CvMat *A,const CvMat *B,const CvMat *C)
{
	CvMat *restemp,*res;
	restemp=MultiplyMatrix(A,B);
	res=MultiplyMatrix(restemp,C);
	return res;
}
CvMat *InvertMatrix(const CvMat *A)
{
	int w=A->width;
	int h=A->height;
	assert(w==h);
	//assert( (A->width!=A->height) );
	CvMat *res=cvCreateMat(A->rows,A->cols,A->type);
	cvInvert(A,res);
	return res;
}

void GetIntrinsicMatrice(CameraParameters p,CvMat *intrinsc)
{
	SetValue(intrinsc,0,0,p.focal.x); 
	SetValue(intrinsc,0,2,p.optical_center.x); 
	SetValue(intrinsc,1,1,p.focal.y); 
	SetValue(intrinsc,1,2,p.optical_center.y); 
	SetValue(intrinsc,2,2,1.0);
	

}
/** *********************return A'***************************************************
  **/
CvMat * GetTransposeMatrix(CvMat *A)
{
	CvMat *res=cvCreateMat(A->width,A->height,CV_64F);
	cvTranspose(A,res);
	return res;
}
void TransformIntrinsicMatrixToCameraParameters(const CvMat* intrinsic,const CvMat *coeffs,CameraParameters &p)
{
	p.focal.x=CV_MAT_ELEM(*intrinsic,double,0,0);
	p.focal.y=CV_MAT_ELEM(*intrinsic,double,1,1);
	
	p.optical_center.x=CV_MAT_ELEM(*intrinsic,double,0,2);
	p.optical_center.y=CV_MAT_ELEM(*intrinsic,double,1,2);

	//BUG
	/*p.k1=CV_MAT_ELEM(*coeffs,double,0,0);
	p.k2=CV_MAT_ELEM(*coeffs,double,0,1);
	p.p1=CV_MAT_ELEM(*coeffs,double,0,2);
	p.p2=CV_MAT_ELEM(*coeffs,double,0,3);
	p.k3=CV_MAT_ELEM(*coeffs,double,0,4);*/
	
	p.k1=CV_MAT_ELEM(*coeffs,double,0,0);
	p.k2=CV_MAT_ELEM(*coeffs,double,1,0);
	p.p1=CV_MAT_ELEM(*coeffs,double,2,0);
	p.p2=CV_MAT_ELEM(*coeffs,double,3,0);
	p.k3=CV_MAT_ELEM(*coeffs,double,4,0);
	
}

void TransformCameraParametersToIntrinsicMatrix(CvMat* intrinsic,CvMat *coeffs,const CameraParameters &p)
{	
	cvZero(intrinsic);
	cvZero(coeffs);
	CV_MAT_ELEM(*intrinsic,double,0,0)=p.focal.x;
	CV_MAT_ELEM(*intrinsic,double,1,1)=p.focal.y;
	
	CV_MAT_ELEM(*intrinsic,double,0,2)=p.optical_center.x;
	CV_MAT_ELEM(*intrinsic,double,1,2)=p.optical_center.y;
	
	CV_MAT_ELEM(*intrinsic,double,2,2)=1;

	CV_MAT_ELEM(*coeffs,double,0,0)=p.k1;
	CV_MAT_ELEM(*coeffs,double,0,1)=p.k2;
	CV_MAT_ELEM(*coeffs,double,0,2)=p.p1;
	CV_MAT_ELEM(*coeffs,double,0,3)=p.p2;
	CV_MAT_ELEM(*coeffs,double,0,4)=p.k3;
	
}


#pragma endregion
/** *********************class StereoMatcher**********************************************************
*************************class StereoMatcher**********************************************************
*************************class StereoMatcher**********************************************************
*************************class StereoMatcher**********************************************************
*************************class StereoMatcher**********************************************************/
StereoMatcher::StereoMatcher()
{


sm_fundMatr = cvCreateMat(3,3,CV_64F);
sm_essentialMatr=cvCreateMat(3,3,CV_64F);

sm_camerasparameters.push_back(cvCreateMat(3,3,CV_64F));
sm_camerasparameters.push_back(cvCreateMat(3,3,CV_64F));

sm_camerasdistortcoeffs.push_back(cvCreateMat(1,5,CV_64F));//sm_camerasdistortcoeffs.push_back(cvCreateMat(5,1,CV_64F));
sm_camerasdistortcoeffs.push_back(cvCreateMat(1,5,CV_64F));//sm_camerasdistortcoeffs.push_back(cvCreateMat(5,1,CV_64F));

for(int i=0;i<4;i++)
sm_PR4[i] = cvCreateMat( 3, 4, CV_64F );

sm_PL=cvCreateMat(3,4,CV_64F);
//set PL to identity matrix
CV_MAT_ELEM(*sm_PL,double,0,0)=1.0;
CV_MAT_ELEM(*sm_PL,double,1,1)=1.0;
CV_MAT_ELEM(*sm_PL,double,2,2)=1.0;


sm_good_R=cvCreateMat(3,1,CV_64F);
sm_good_T=cvCreateMat(3,1,CV_64F);
sm_good_RotMatrix=cvCreateMat(3,3,CV_64F);
sm_found_good_matrix=false;


sm_point3dreconstructor=new Point3dReconstructor();

}

 /** *********************destructeur****************************************************
  **/
StereoMatcher::~StereoMatcher()
{


	cvReleaseMat(&sm_status);
	cvReleaseMat(&sm_fundMatr);
	cvReleaseMat(&sm_essentialMatr);

	for(int i=0;i<4;i++)
		cvReleaseMat(&sm_PR4[i]);
	
	cvReleaseMat(&sm_PL);

	for(int i=0;i<sm_camerasdistortcoeffs.size();i++)
	{
		cvReleaseMat(&sm_camerasparameters[i]);
		cvReleaseMat(&sm_camerasdistortcoeffs[i]);
	}
		sm_camerasparameters.empty();
	sm_camerasdistortcoeffs.empty();
	

	cvReleaseMat(&sm_good_R);
	cvReleaseMat(&sm_good_T);
	cvReleaseMat(&sm_good_RotMatrix);
	delete sm_point3dreconstructor;
}

 /** *********************load left and right 2d points containers****************************************************
  **/
void StereoMatcher::Load2dPoints(const  std::vector<CvPoint > & left_points,const std::vector<CvPoint > & right_points)
{
	assert(left_points.size()==right_points.size());
	
	sm_numpoints=left_points.size();
	sm_points[0].assign(sm_numpoints,CvPoint2D32f());
	sm_points[1].assign(sm_numpoints,CvPoint2D32f());

#ifdef ESTIMATE_ESSENTIAL_DIRECTLY
	float left_fx=CV_MAT_ELEM(*sm_camerasparameters[0],float,0,0);
	float left_fy=CV_MAT_ELEM(*sm_camerasparameters[0],float,1,1);

	float left_cx=CV_MAT_ELEM(*sm_camerasparameters[0],float,0,2);
	float left_cy=CV_MAT_ELEM(*sm_camerasparameters[0],float,1,2);
	
	float right_fx=CV_MAT_ELEM(*sm_camerasparameters[1],float,0,0);
	float right_fy=CV_MAT_ELEM(*sm_camerasparameters[1],float,1,1);

	float right_cx=CV_MAT_ELEM(*sm_camerasparameters[1],float,0,2);
	float right_cy=CV_MAT_ELEM(*sm_camerasparameters[1],float,1,2);
#endif

	for (int i = 0; i < left_points.size(); i++)
	{	
#ifndef ESTIMATE_ESSENTIAL_DIRECTLY

		sm_points[0][i].x=left_points[i].x;
		sm_points[0][i].y=left_points[i].y;

		sm_points[1][i].x=right_points[i].x;
		sm_points[1][i].y=right_points[i].y;
#else
		sm_points[0][i].x=(left_points[i].x-left_cx)/left_fx;
		sm_points[0][i].y=(left_points[i].y-left_cy)/left_fy;

		sm_points[1][i].x=(right_points[i].x-right_cx)/right_fx;
		sm_points[1][i].y=(right_points[i].y-right_cy)/right_fy;

#endif
	}

	sm_points_left=cvMat(1,sm_numpoints,CV_32FC2,&sm_points[0][0]);
	sm_points_right=cvMat(1,sm_numpoints,CV_32FC2,&sm_points[1][0]);
	//PrintMatrix(&sm_points_left,"points left");
	//PrintMatrix(&sm_points_right,"points right");

}

/** *********************revoit la matrice fundamentale trouve****************************************************
  **/
int  StereoMatcher::Analyse()
{	


	
 	int num = FindFundamentalMatrix();
	cout<<"\nNumber of fundamental matrix found : "<<num<<"\n";
	if(num)
				{	
				
					ValidateFundamentalMatrix();
					FindEssentialMatrix();
					DecomposeEssentialMatrix();
					ConstructTheFourProjectionMatrix();
					SelectGoodRightProjectionMatrix();
					return sm_found_good_matrix?1:0;
				}
	//else cout<<"Fundamental matrix could not be evaluated...\n";

	
	else {cout<<"\nNo fundamental matrix found... \n";return -1;}




}



/** *********************Get Fundamental Matrix****************************************************
  **/
int StereoMatcher::FindFundamentalMatrix()
{	


	sm_status=cvCreateMat(1,this->sm_numpoints,CV_8UC1);
	int res=cvFindFundamentalMat(&sm_points_left,&sm_points_right,sm_fundMatr,CV_FM_RANSAC,3.0,0.99,sm_status);//CV_FM_LMEDS	CV_FM_RANSAC	CV_FM_8POINT	CV_FM_7POINT
	PrintMatrix(sm_fundMatr,"Fundamental Matrix");
	return res;
	
	
}



/** *********************validate fundamental matrix***********************************************
  **/
void StereoMatcher::ValidateFundamentalMatrix()
{
	
	

	double determinant=cvDet(sm_fundMatr);
	cout<<"deterimant of fundamental matrix:"<<determinant<<"\n";

	sm_lines[0].resize(sm_numpoints);
	sm_lines[1].resize(sm_numpoints);

	CvMat L1 = cvMat(1, sm_numpoints, CV_32FC3, &sm_lines[0][0]);
	CvMat L2 = cvMat(1, sm_numpoints, CV_32FC3, &sm_lines[1][0]);



	cvComputeCorrespondEpilines(&sm_points_left,1,sm_fundMatr,&L1);
	cvComputeCorrespondEpilines(&sm_points_right, 2,sm_fundMatr, &L2 );
	
	
	double avgErr = 0;
	for( int i = 0; i < sm_numpoints; i++ )
	{
		double err1 = fabs(sm_points[0][i].x*sm_lines[1][i].x +
		sm_points[0][i].y*sm_lines[1][i].y + sm_lines[1][i].z);
		printf("point %d on left camera has error: %f\n",i,err1);
		
		double err2= fabs(sm_points[1][i].x*sm_lines[0][i].x +
		sm_points[1][i].y*sm_lines[0][i].y + sm_lines[0][i].z);
		printf("point :%d on right camera has error: %f\n",i,err2);

		avgErr += err1+err2;

	}

	printf( "avg err = %g\n", avgErr/(sm_numpoints) );	
	
	//lines[0].empty();
	//lines[1].empty();


}



//debug fundamental matrix
#ifdef DEBUG_FUNDAMENTAL_MATRIX
CvPoint2D32f ptr;
IplImage *LI,*RI;
CvMat *FM;
void On_MouseDebug(int event, int x, int y, int flags, void* param)
{
		
	switch(event)
	{
	case CV_EVENT_LBUTTONDOWN:
		{	
			std::vector<CvPoint3D32f> line;
			line.resize(1);
			CvMat L1= cvMat(1, 1, CV_32FC3, &line[0]);
			

			ptr = cvPoint2D32f(x,y);
			printf("(%f,%f)",ptr.x,ptr.y);
			CvFont font;
			cvInitFont(&font,CV_FONT_HERSHEY_PLAIN|CV_FONT_ITALIC,1,1,0,1);
			cvCircle(LI,cvPoint((int)ptr.x,(int)ptr.y),1,cvScalar(0,0,255),3);
			
			
			CvMat left_point=cvMat(1,1,CV_32FC2,&ptr);
			float t=CV_MAT_ELEM(left_point,float,0,0);
			float t1=CV_MAT_ELEM(left_point,float,0,1);

			cvComputeCorrespondEpilines(&left_point,1,FM,&L1);
			cvLine(RI,cvPoint(0,-line[0].z/line[0].y),cvPoint(-line[0].z/line[0].x,0),CV_RGB(0,0,255));
			
			cvShowImage("LEFT", LI);
			cvShowImage("Epilolar lines", RI);
			

		}
		break;
	}



}
#endif

void StereoMatcher::DrawCorrespondingEpilines(IplImage *left_images,IplImage *right_images)
{	
	cvNamedWindow("Epilolar lines");
	cvNamedWindow("lEFT");

 	for(int i=0;i<sm_numpoints;i++)
	{
	cvLine(right_images,cvPoint(0,-sm_lines[0][i].z/sm_lines[0][i].y),cvPoint(-sm_lines[0][i].z/sm_lines[0][i].x,0),CV_RGB(0,255,0));
	//cout<<"a="<<sm_lines[0][i].x<<"\tb="<<sm_lines[0][i].y<<"\tc="<<sm_lines[0][i].z<<"\n";
	cvShowImage("Epilolar lines",right_images);
	cvShowImage("lEFT",left_images);
	cvWaitKey(25);

	}
	
   	cvDestroyAllWindows();

	
	//debug fundamental matrix
#ifdef DEBUG_FUNDAMENTAL_MATRIX
	FM=cvCreateMat(3,3,CV_32F);
	LI=left_images;
	RI=right_images;
	FM=sm_fundMatr;

	while(1)
	{
	
	cvNamedWindow("LEFT", CV_WINDOW_AUTOSIZE);
	cvSetMouseCallback("LEFT",On_MouseDebug,0);

	cvNamedWindow("Epilolar lines", CV_WINDOW_AUTOSIZE);
	cvSetMouseCallback("Epilolar lines",On_MouseDebug,0);

	cvShowImage("lEFT",LI);
	cvShowImage("Epilolar lines", RI);
	cvWaitKey(0);
	

   	}
#endif


	
}

/** *********************Get essential Matrix****************************************************
  **/
int StereoMatcher::FindEssentialMatrix()
{

#ifndef ESTIMATE_ESSENTIAL_DIRECTLY
	CvMat *transC2;
	CvMat *temp;
	transC2=GetTransposeMatrix(sm_camerasparameters[1]);
	
	temp=MultiplyMatrix(transC2,sm_fundMatr);
	sm_essentialMatr=MultiplyMatrix(temp,sm_camerasparameters[0]);
	
	cvScale( sm_essentialMatr, sm_essentialMatr, 1.0 / cvmGet(sm_essentialMatr, 1, 2 ) );


	cvReleaseMat( &temp );
	cvReleaseMat( &transC2 );
#else
	sm_essentialMatr=sm_fundMatr;
#endif
	PrintMatrix(sm_essentialMatr,"Essential Matrix");

	return 1;

}


/** *********************get matching points nb****************************************************
  **/
int StereoMatcher::GetPointsNumber()
{
	return sm_numpoints;
}


/** *********************get camera parameters****************************************************
  **/
void StereoMatcher::SetCamerasParameters(const CameraParameters &c_left,const CameraParameters &c_right)
{	

	Stereo::TransformCameraParametersToIntrinsicMatrix(sm_camerasparameters[0],sm_camerasdistortcoeffs[0],c_left);
	Stereo::TransformCameraParametersToIntrinsicMatrix(sm_camerasparameters[1],sm_camerasdistortcoeffs[1],c_right);
	//GetIntrinsicMatrice(c_left,sm_camerasparameters[0]);
	//GetIntrinsicMatrice(c_right,sm_camerasparameters[1]);
	
	//construct left projection matrix K*I where K is the 3*3 intrinsic matrix and I is a 4*4 identity matrix
	#ifndef ESTIMATE_ESSENTIAL_DIRECTLY
	sm_PL=MultiplyMatrix( sm_camerasparameters[0],sm_PL);
	#endif


	PrintMatrix(sm_camerasparameters[0],"I1");
	PrintMatrix(sm_camerasparameters[1],"I2");


}

/** *********************(U,V,W)=SVD(E)***************************************************
  **/
void StereoMatcher::DecomposeEssentialMatrix()
{	
	
	
	CvMat *SV=cvCreateMat(sm_essentialMatr->width,sm_essentialMatr->height,CV_64F);
	CvMat *W=cvCreateMat(sm_essentialMatr->width,sm_essentialMatr->height,CV_64F);
	CvMat *U=cvCreateMat(sm_essentialMatr->width,sm_essentialMatr->height,CV_64F);
	CvMat *V=cvCreateMat(sm_essentialMatr->width,sm_essentialMatr->height,CV_64F);
	CvMat *S=cvCreateMat(sm_essentialMatr->width,sm_essentialMatr->height,CV_64F);
	
	
	//Rotation matrix and  translation matrix 
	CvMat* t  = cvCreateMat( 3, 1, CV_64F );
	CvMat* Ra = cvCreateMat( 3, 3, CV_64F );
	CvMat* Rb = cvCreateMat( 3, 3, CV_64F );


	cvSVD( sm_essentialMatr, SV, U, V );
	


	cvmSet( SV, 2, 2, 0.0 );
	float v = ( (float)cvmGet( SV, 0, 0 ) + (float)cvmGet( SV, 1, 1 ) ) / 2.0;
	cvmSet( SV, 0, 0, v );
	cvmSet( SV, 1, 1, v );
	PrintMatrix(SV,"SV");


	sm_essentialMatr=MultiplyMatrix(U,SV,GetTransposeMatrix(V) );

	

	cvScale(sm_essentialMatr,sm_essentialMatr,1.0f/v);



	cvZero(SV);
	cvZero(U);
	cvZero(V);

	cvSVD(sm_essentialMatr,SV,U,V );
	
	//PrintMatrix(SV,"SV");
	//PrintMatrix(U,"U");
	//PrintMatrix(V,"V");

	CvMat *Z=cvCreateMat(sm_essentialMatr->width,sm_essentialMatr->height,CV_64F);
	cvZero(Z);
	cvmSet( Z, 0, 1,  1.0 );
	cvmSet( Z, 1, 0, -1.0 );


	S=MultiplyMatrix(U,Z,GetTransposeMatrix(U) );

	for( int i=0; i<3; i++ )
		cvmSet( t, i, 0, cvmGet( U, i, 2 ) );

	
	cvZero( W );
	cvmSet( W, 0, 1, -1.0 );
	cvmSet( W, 1, 0,  1.0 );
	cvmSet( W, 2, 2,  1.0 );
		
	Ra=MultiplyMatrix( U, W, GetTransposeMatrix(V) );
	W=GetTransposeMatrix(W);
	Rb=MultiplyMatrix( U, W, GetTransposeMatrix(V) );
	


	if( cvDet( Ra ) < 0 )
		cvScale( Ra, Ra, -1.0 );
	if( cvDet( Rb ) < 0 )
		cvScale( Rb, Rb, -1.0 );


	sm_Ra=Ra;
	sm_Rb=Rb;
	sm_T=t;

	cvReleaseMat( &W );
	cvReleaseMat( &Z );
	cvReleaseMat( &U );
	cvReleaseMat( &V );
	cvReleaseMat( &S );
	cvReleaseMat( &SV );

	
	PrintMatrix(sm_Ra,"Ra");
	PrintMatrix(sm_Rb,"Rb");

	CvMat *ra_vec=cvCreateMat(3,1,CV_64FC1);
	CvMat *rb_vec=cvCreateMat(3,1,CV_64FC1);
	cvRodrigues2(Ra,ra_vec);
	cvRodrigues2(Rb,rb_vec);

	Stereo::PrintMatrix(ra_vec,"Ra_vector");
	Stereo::PrintMatrix(rb_vec,"Rb_vector");
	Stereo::PrintMatrix(sm_T,"T1");

}
/************************* i=0 [Ra | T]		i=1	[Ra | -T]	i=2	[Rb | T]	i=3	[Rb | -T]****************************
  **/
void StereoMatcher::ConstructTheFourProjectionMatrix()
{

	for( int i=0; i<4; i++ )	
		cvSetIdentity( sm_PR4[i] );

	//copy translation vector first
	for( int i=0; i<4; i++ )	
		for( int li=0; li<3; li++ ) 
			{
				float val = cvmGet( sm_T, li, 0 );
				if(i%2)		val = -val;
				cvmSet( sm_PR4[i], li, 3, val );
			}	
	
	//copy rotation matrix
	for( int i=0; i<4; i++ )
	{
		for( int row=0; row<3; row++ )
		{	

			for( int col=0; col<3; col++ )
			{	
				if(i<2)	CV_MAT_ELEM(*sm_PR4[i],double,row,col)=CV_MAT_ELEM(*sm_Ra,double,row,col);
				else	CV_MAT_ELEM(*sm_PR4[i],double,row,col)=CV_MAT_ELEM(*sm_Rb,double,row,col);
			}
		}
	}

	//\\todo ? 
	cout<<"the four projection matrix are : ";
	for(int i=0;i<4;i++)
		PrintMatrix(sm_PR4[i],"Projection matrix");

	//construct the four projection matrix for right camera .
	#ifndef ESTIMATE_ESSENTIAL_DIRECTLY
	for( int i=0; i<4; i++ )
		cvMatMul( sm_camerasparameters[1], sm_PR4[i], sm_PR4[i] );
	#endif

}
void StereoMatcher::SelectGoodRightProjectionMatrix()
{
	CvPoint3D32f X[4];

	CvPoint3D32f X_L[4];
	CvPoint3D32f X_R[4];
	
	CvMat *invT=cvCreateMat(3,1,CV_64F);
	cvScale(sm_T,invT,-1.0f);
	
	
	int r1,r2,r3,r4;
	
		//sm_point3dreconstructor->Reconstruct3dPoint(sm_points[0][0],sm_points[1][0],X[i],sm_PL,sm_PR4[i]);
		r1=sm_point3dreconstructor->Reconstruct3dpointTriangulation(sm_points[0][0],sm_points[1][0],X_L[0],X_R[0],sm_camerasparameters[0],sm_camerasparameters[1],sm_Ra,sm_T);
		r2=sm_point3dreconstructor->Reconstruct3dpointTriangulation(sm_points[0][0],sm_points[1][0],X_L[1],X_R[1],sm_camerasparameters[0],sm_camerasparameters[1],sm_Ra,invT);
		r3=sm_point3dreconstructor->Reconstruct3dpointTriangulation(sm_points[0][0],sm_points[1][0],X_L[2],X_R[2],sm_camerasparameters[0],sm_camerasparameters[1],sm_Rb,sm_T);
		r4=sm_point3dreconstructor->Reconstruct3dpointTriangulation(sm_points[0][0],sm_points[1][0],X_L[3],X_R[3],sm_camerasparameters[0],sm_camerasparameters[1],sm_Rb,invT);
		//cout<<"X[ "<<i<<"] : "<<X[i].x<<"\t"<<X[i].y<<"\t"<<X[i].z<<" \n";
		
	for(int i=0;i<4;i++)
	{
		cout<<"X_R[ "<<i<<"] : "<<X_R[i].x<<"\t"<<X_R[i].y<<"\t"<<X_R[i].z<<" \n";
		cout<<"X_L[ "<<i<<"] : "<<X_L[i].x<<"\t"<<X_L[i].y<<"\t"<<X_L[i].z<<" \n";
		
	}

	if(r1==0) {sm_good_RotMatrix=sm_Ra;cvRodrigues2(sm_Ra,sm_good_R);sm_good_T=sm_T;sm_found_good_matrix=true;return;}
	if(r2==0) {sm_good_RotMatrix=sm_Ra;cvRodrigues2(sm_Ra,sm_good_R);sm_good_T=invT;sm_found_good_matrix=true;return;}
	if(r3==0) {sm_good_RotMatrix=sm_Rb;cvRodrigues2(sm_Rb,sm_good_R);sm_good_T=sm_T;sm_found_good_matrix=true;return;}
	if(r4==0) {sm_good_RotMatrix=sm_Rb;cvRodrigues2(sm_Rb,sm_good_R);sm_good_T=invT;sm_found_good_matrix=true;return;}


}
void StereoMatcher::GetGoodRightProjectionMatrix(CvMat *Rmat,CvMat *R,CvMat *T,CvMat *Essential,CvMat *Fundamental)
{
	if(sm_found_good_matrix)
	{

	//copy to output
	
	//translation vector
	CV_MAT_ELEM(*T,double,0,0)=CV_MAT_ELEM(*sm_good_T,double,0,0);
	CV_MAT_ELEM(*T,double,1,0)=CV_MAT_ELEM(*sm_good_T,double,1,0);
	CV_MAT_ELEM(*T,double,2,0)=CV_MAT_ELEM(*sm_good_T,double,2,0);
	
	//rotation vector
	CV_MAT_ELEM(*R,double,0,0)=CV_MAT_ELEM(*sm_good_R,double,0,0);
	CV_MAT_ELEM(*R,double,1,0)=CV_MAT_ELEM(*sm_good_R,double,1,0);
	CV_MAT_ELEM(*R,double,2,0)=CV_MAT_ELEM(*sm_good_R,double,2,0);

	//rotation matrix
	CV_MAT_ELEM(*Rmat,double,0,0)=CV_MAT_ELEM(*sm_good_RotMatrix,double,0,0);
	CV_MAT_ELEM(*Rmat,double,0,1)=CV_MAT_ELEM(*sm_good_RotMatrix,double,0,1);
	CV_MAT_ELEM(*Rmat,double,0,2)=CV_MAT_ELEM(*sm_good_RotMatrix,double,0,2);

	CV_MAT_ELEM(*Rmat,double,1,0)=CV_MAT_ELEM(*sm_good_RotMatrix,double,1,0);
	CV_MAT_ELEM(*Rmat,double,1,1)=CV_MAT_ELEM(*sm_good_RotMatrix,double,1,1);
	CV_MAT_ELEM(*Rmat,double,1,2)=CV_MAT_ELEM(*sm_good_RotMatrix,double,1,2);

	CV_MAT_ELEM(*Rmat,double,2,0)=CV_MAT_ELEM(*sm_good_RotMatrix,double,2,0);
	CV_MAT_ELEM(*Rmat,double,2,1)=CV_MAT_ELEM(*sm_good_RotMatrix,double,2,1);
	CV_MAT_ELEM(*Rmat,double,2,2)=CV_MAT_ELEM(*sm_good_RotMatrix,double,2,2);

	//essential matrix
	CV_MAT_ELEM(*Essential,double,0,0)=CV_MAT_ELEM(*sm_essentialMatr,double,0,0);
	CV_MAT_ELEM(*Essential,double,0,1)=CV_MAT_ELEM(*sm_essentialMatr,double,0,1);
	CV_MAT_ELEM(*Essential,double,0,2)=CV_MAT_ELEM(*sm_essentialMatr,double,0,2);

	CV_MAT_ELEM(*Essential,double,1,0)=CV_MAT_ELEM(*sm_essentialMatr,double,1,0);
	CV_MAT_ELEM(*Essential,double,1,1)=CV_MAT_ELEM(*sm_essentialMatr,double,1,1);
	CV_MAT_ELEM(*Essential,double,1,2)=CV_MAT_ELEM(*sm_essentialMatr,double,1,2);

	CV_MAT_ELEM(*Essential,double,2,0)=CV_MAT_ELEM(*sm_essentialMatr,double,2,0);
	CV_MAT_ELEM(*Essential,double,2,1)=CV_MAT_ELEM(*sm_essentialMatr,double,2,1);
	CV_MAT_ELEM(*Essential,double,2,2)=CV_MAT_ELEM(*sm_essentialMatr,double,2,2);

	//fundamental matrix
	CV_MAT_ELEM(*Fundamental,double,0,0)=CV_MAT_ELEM(*sm_fundMatr,double,0,0);
	CV_MAT_ELEM(*Fundamental,double,0,1)=CV_MAT_ELEM(*sm_fundMatr,double,0,1);
	CV_MAT_ELEM(*Fundamental,double,0,2)=CV_MAT_ELEM(*sm_fundMatr,double,0,2);

	CV_MAT_ELEM(*Fundamental,double,1,0)=CV_MAT_ELEM(*sm_fundMatr,double,1,0);
	CV_MAT_ELEM(*Fundamental,double,1,1)=CV_MAT_ELEM(*sm_fundMatr,double,1,1);
	CV_MAT_ELEM(*Fundamental,double,1,2)=CV_MAT_ELEM(*sm_fundMatr,double,1,2);

	CV_MAT_ELEM(*Fundamental,double,2,0)=CV_MAT_ELEM(*sm_fundMatr,double,2,0);
	CV_MAT_ELEM(*Fundamental,double,2,1)=CV_MAT_ELEM(*sm_fundMatr,double,2,1);
	CV_MAT_ELEM(*Fundamental,double,2,2)=CV_MAT_ELEM(*sm_fundMatr,double,2,2);




	PrintMatrix(R,"Final R");
	PrintMatrix(T,"Final T");
	PrintMatrix(Rmat,"Final R matrix");
	PrintMatrix(Essential,"Final Essential matrix");
	PrintMatrix(Fundamental,"Final Fundamental matrix");

	}
	else std::cout<<"No projection found between the two cameras\n.";
	
	
}
void StereoMatcher::ReconstructTrinagulation3DPointWithGoodProjectionMatrixTrinagulation(
														const CvPoint2D32f & Left2DPoint,
														const CvPoint2D32f & Right2DPoint,
														CvPoint3D32f & leftpoint3d,
														CvPoint3D32f & rightpoint3d
														)
{
	if(sm_found_good_matrix)

	this->sm_point3dreconstructor->Reconstruct3dpointTriangulation(
		Left2DPoint,
		Right2DPoint,
		leftpoint3d,
		rightpoint3d,
		sm_camerasparameters[0],
		sm_camerasparameters[1],
		sm_good_RotMatrix,
		sm_good_T);
	else cout<<"Could not reconstruct 3d point because there is no good projection matrix between the two cameras\n";
	

	
}

/** *********************class Point3dReconstructor**********************************************************
*************************class Point3dReconstructor**********************************************************
*************************class Point3dReconstructor**********************************************************
*************************class Point3dReconstructor**********************************************************
*************************class Point3dReconstructor**********************************************************/

Point3dReconstructor::Point3dReconstructor()
{

	InitHomogenous();
	InitTriangulation();

}
Point3dReconstructor::~Point3dReconstructor()
{
	cvReleaseMat( &P3DREC_A );			P3DREC_A			= NULL;
	cvReleaseMat( &P3DREC_X );			P3DREC_X			= NULL;
	cvReleaseMat( &P3DREC_W );			P3DREC_W			= NULL;
	cvReleaseMat( &P3DREC_U );			P3DREC_U			= NULL;
	cvReleaseMat( &P3DREC_V );			P3DREC_V			= NULL;


	cvReleaseMat( &P3DREC_A_);			P3DREC_A_			= NULL;
	cvReleaseMat( &P3DREC_A1);			P3DREC_A1			= NULL;
	cvReleaseMat( &P3DREC_invAtA);		P3DREC_invAtA		= NULL;
	cvReleaseMat( &P3DREC_invAtAAt);	P3DREC_invAtAAt		= NULL;
	cvReleaseMat( &P3DREC_res_Z);		P3DREC_res_Z		= NULL;


}
//reconstruct 3d point using homogeneous method or triangulation
void Point3dReconstructor::Reconstruct3dPointHomogeous(
							const CvPoint2D32f & Left2DPoint,
							const CvPoint2D32f & Right2DPoint, 
							CvPoint3D32f & point3d,
							CvMat *LeftProjectionMatrix,
							CvMat * RightProjectionMatrix
							)
{	
	float xL=Left2DPoint.x;
	float yL=Left2DPoint.y;

	float xR=Right2DPoint.x;
	float yR=Right2DPoint.y;
	

	//homogeneous method

	
		//fill A
		for( int i=0; i<4; i++ )
		{
			cvmSet( P3DREC_A, 0, i, xL * cvmGet( LeftProjectionMatrix, 2, i ) - cvmGet( LeftProjectionMatrix, 0, i ) );
			cvmSet( P3DREC_A, 1, i, yL * cvmGet( LeftProjectionMatrix, 2, i ) - cvmGet( LeftProjectionMatrix, 1, i ) );
			cvmSet( P3DREC_A, 2, i, xR * cvmGet( RightProjectionMatrix, 2, i )- cvmGet( RightProjectionMatrix, 0, i ) );
			cvmSet( P3DREC_A, 3, i, yR * cvmGet( RightProjectionMatrix, 2, i )- cvmGet( RightProjectionMatrix, 1, i ) );
		}

	//get singular values of A 
	cvSVD( P3DREC_A, P3DREC_W, P3DREC_U, P3DREC_V );

	// the last colum of V is the lowest eagen values of A which are the right solution 
	for( int i=0; i<4; i++ )
		cvmSet( P3DREC_X, i, 0, cvmGet( P3DREC_V, i, 3 ) );

	
	
	//get the final 3d point
	double v = cvmGet( P3DREC_X, 3, 0 );
	point3d.x	=	( cvmGet( P3DREC_X, 0, 0 ) / v );
	point3d.y	=	( cvmGet( P3DREC_X, 1, 0 ) / v );
	point3d.z	=	( cvmGet( P3DREC_X, 2, 0 ) / v );



}
void Point3dReconstructor::InitHomogenous()
{	
	int rows = 4; //A->rows;
	int cols = 4; //A->cols;

	P3DREC_A   = cvCreateMat( 4, 4, CV_64F );
	P3DREC_X   = cvCreateMat( 4, 1, CV_64F );
	P3DREC_W   = cvCreateMat( rows, cols, CV_64F );
	P3DREC_U   = cvCreateMat( rows, cols, CV_64F );
	P3DREC_V   = cvCreateMat( rows, cols, CV_64F );
}
void Point3dReconstructor::InitTriangulation()
{

	P3DREC_A_=cvCreateMat(3,2,CV_64F);
	P3DREC_A1=cvCreateMat(3,1,CV_64F);

	P3DREC_invAtA=cvCreateMat(2,2,CV_64F);
	P3DREC_invAtAAt=cvCreateMat(2,3,CV_64F);
	P3DREC_res_Z=cvCreateMat(2,1,CV_64F);

}
int Point3dReconstructor::Reconstruct3dpointTriangulation(
															const CvPoint2D32f & Left2DPoint,
															const CvPoint2D32f & Right2DPoint,
															CvPoint3D32f & leftpoint3d,
															CvPoint3D32f & rightpoint3d,
															const CvMat *leftintrinsic,
															const CvMat *rightintrinsic,
															const CvMat *RotationMatrix,
															const CvMat *Translationvector
	)
{

	//normalize coordinates
	
	double _xL=((double)Left2DPoint.x-CV_MAT_ELEM(*leftintrinsic,double,0,2))/CV_MAT_ELEM(*leftintrinsic,double,0,0);
	double _yL=((double)Left2DPoint.y-CV_MAT_ELEM(*leftintrinsic,double,1,2))/CV_MAT_ELEM(*leftintrinsic,double,1,1);

	double _xR=((double)Right2DPoint.x-CV_MAT_ELEM(*rightintrinsic,double,0,2))/(float)CV_MAT_ELEM(*rightintrinsic,double,0,0);
	double _yR=((double)Right2DPoint.y-CV_MAT_ELEM(*rightintrinsic,double,1,2))/(float)CV_MAT_ELEM(*rightintrinsic,double,1,1);
 


	//CvMat *A=cvCreateMat(3,2,CV_32F);
	//CvMat *A1=cvCreateMat(3,1,CV_32F);
	CvMat *left_2dpoint=cvCreateMat(3,1,CV_64F);

	//CvMat *invAtA=cvCreateMat(2,2,CV_32F);
	//CvMat *invAtAAt=cvCreateMat(2,3,CV_32F);
	//CvMat *res_Z=cvCreateMat(2,1,CV_32F);

	cvZero(left_2dpoint);
	cvZero(P3DREC_A_);
	cvZero(P3DREC_A1);
	
	CV_MAT_ELEM(*left_2dpoint,double,0,0)=_xL;
	CV_MAT_ELEM(*left_2dpoint,double,1,0)=_yL;
	CV_MAT_ELEM(*left_2dpoint,double,2,0)=1;

	
	CV_MAT_ELEM(*P3DREC_A_,double,0,0)=_xR;
	CV_MAT_ELEM(*P3DREC_A_,double,1,0)=_yR;
	CV_MAT_ELEM(*P3DREC_A_,double,2,0)=1;

	P3DREC_A1=MultiplyMatrix(RotationMatrix,left_2dpoint);
	cvScale(P3DREC_A1,P3DREC_A1,-1.0);

	CV_MAT_ELEM(*P3DREC_A_,double,0,1)=CV_MAT_ELEM(*P3DREC_A1,double,0,0);
	CV_MAT_ELEM(*P3DREC_A_,double,1,1)=CV_MAT_ELEM(*P3DREC_A1,double,1,0);
	CV_MAT_ELEM(*P3DREC_A_,double,2,1)=CV_MAT_ELEM(*P3DREC_A1,double,2,0);

	
	P3DREC_invAtA= InvertMatrix( MultiplyMatrix(GetTransposeMatrix(P3DREC_A_),P3DREC_A_) );
	//cvInvert( MultiplyMatrix(GetTransposeMatrix(P3DREC_A_),P3DREC_A_),P3DREC_invAtA );
	
	P3DREC_invAtAAt=MultiplyMatrix(P3DREC_invAtA,GetTransposeMatrix(P3DREC_A_));
	P3DREC_res_Z=MultiplyMatrix(P3DREC_invAtAAt,Translationvector);

	//PrintMatrix(P3DREC_res_Z,"Z");
	
	float _zR=(float)CV_MAT_ELEM(*P3DREC_res_Z,double,0,0);
	float _zL=(float)CV_MAT_ELEM(*P3DREC_res_Z,double,1,0);

	
	rightpoint3d.x=_xR*_zR;
	rightpoint3d.y=_yR*_zR;
	rightpoint3d.z=_zR;

	leftpoint3d.x=_xL*_zL;
	leftpoint3d.y=_yL*_zL;
	leftpoint3d.z=_zL;

	if(_zR>=0 && _zL>=0) return 0;
	else return -1;



}


int Point3dReconstructor::Reconstruct3dpointTriangulation(
		const CvPoint2D32f & Left2DPoint,
		const CvPoint2D32f & Right2DPoint,
		CvPoint3D32f & leftpoint3d,
		CvPoint3D32f & rightpoint3d,
		const CameraParameters& left_CameraParameters,
		const CameraParameters& right_CameraParameters,
		const CvMat *RotationMatrix,
		const CvMat *Translationvector)
{
	CvMat *left_intrinsics=cvCreateMat(3,3,CV_64F);
	CvMat *left_coeffs=cvCreateMat(1,5,CV_64F);

	CvMat *right_intrinsics=cvCreateMat(3,3,CV_64F);
	CvMat *right_coeffs=cvCreateMat(1,5,CV_64F);

	Stereo::TransformCameraParametersToIntrinsicMatrix(left_intrinsics,left_coeffs,left_CameraParameters);
	Stereo::TransformCameraParametersToIntrinsicMatrix(right_intrinsics,right_coeffs,right_CameraParameters);
	return Reconstruct3dpointTriangulation(Left2DPoint,Right2DPoint,leftpoint3d,rightpoint3d,left_intrinsics,right_intrinsics,RotationMatrix,Translationvector);

}
/** *********************class POSIT**********************************************************
*************************class POSIT**********************************************************
*************************class POSIT**********************************************************
*************************class POSIT**********************************************************
*************************class POSIT**********************************************************/
POSIT::POSIT()
{

po_intrinsics.push_back(cvCreateMat(3,3,CV_32F));
po_extrinsics.push_back(cvCreateMat(4, 4, CV_32F));

po_rotation_matrix = new float[9];
po_translation_vector = new float[3];

}

POSIT::POSIT(Dimension dimension,CameraParameters camparam,int nbpoints,std::vector<CvPoint3D32f > points)
{
	
	po_rotation_matrix = new float[9];
	po_translation_vector = new float[3];

	this->SetObjectDimension(dimension);
	
	//po_intrinsics = cvCreateMat( 3, 3, CV_64F );
	po_intrinsics.push_back(cvCreateMat(3,3,CV_32F));

	po_extrinsics.push_back(cvCreateMat(4, 4, CV_32F));

	this->InitializeIntrinsics(camparam);

	this->GetObjetPoints(nbpoints,points);

	

}
POSIT::~POSIT()
{
	//if(po_positObject!=NULL) cvReleasePOSITObject(&po_positObject);
	cvReleaseMat(&po_intrinsics[0]);
	cvReleaseMat(&po_extrinsics[0]);
	po_modelPoints.~vector();
	po_imagePoints.~vector();
	delete [] po_rotation_matrix;
	delete [] po_translation_vector;
	

}
void POSIT::SetObjectDimension(Dimension dimension)
{	
	po_objectdimension.dimensionx=dimension.dimensionx;
	po_objectdimension.dimensiony=dimension.dimensiony;
	po_objectdimension.dimensionz=dimension.dimensionz;


}
void POSIT::InitializeIntrinsics( CameraParameters camparam )
{	
	cvSetZero( po_intrinsics[0] );
	GetIntrinsicMatrice(camparam,po_intrinsics[0]);
	PrintMatrix(po_intrinsics[0],"I1");


}


void POSIT::EstimateObjectPos()
{
	int i=0;
	int j=0;
	

	float focal_length = ( (float*)(po_intrinsics[0]->data.ptr + po_intrinsics[0]->step*i) )[j];
	CvTermCriteria criteria = cvTermCriteria(CV_TERMCRIT_EPS, 1000, 1.0e-5);
	cvPOSIT( po_positObject, &po_imagePoints[0], focal_length, criteria, po_rotation_matrix, po_translation_vector );	

	CreateOpenGLMatrix(po_rotation_matrix,po_translation_vector);
	CreateMatrix(po_rotation_matrix,po_translation_vector);
	/*
	printf("\n\n");
	for(int i=0;i<9;i++)
		printf("r[%d]=%f",i+1,po_rotation_matrix[i]);
	printf("\n\n");
	for(int i=0;i<3;i++)
		printf("t[%d]=%f",i+1,po_translation_vector[i]);
		*/
	PrintMatrix(po_extrinsics[0],"Extrinsic");
	
	
}
void POSIT::CreateOpenGLMatrix(const CvMatr32f &rotationMatrix, const CvVect32f &translationVector)
{	

	

	//coordinate system returned is relative to the first 3D input point	
	for (int f=0; f<3; f++)
	{
		for (int c=0; c<3; c++)
		{
			po_posePOSIT[c*4+f] = po_rotation_matrix[f*3+c];	//transposed
		}
	}	
	po_posePOSIT[3] = 0.0;
	po_posePOSIT[7] = 0.0;	
	po_posePOSIT[11] = 0.0;
	po_posePOSIT[12] = po_translation_vector[0];
	po_posePOSIT[13] = po_translation_vector[1]; 
	po_posePOSIT[14] = -po_translation_vector[2]; //negative
	po_posePOSIT[15] = 1.0; //homogeneous
	for(int i=0;i<16;i++)
		printf("r[%d]=%f\n",i,po_posePOSIT[i]);
}
void POSIT::CreateMatrix(const CvMatr32f &rotationMatrix, const CvVect32f &translationVector)
{	
	cvSetZero( po_extrinsics[0] );
	
	SetValue(po_extrinsics[0],0,0,rotationMatrix[0]);
		
	SetValue(po_extrinsics[0],0,1,rotationMatrix[1]); 
	SetValue(po_extrinsics[0],0,2,rotationMatrix[2]);
	SetValue(po_extrinsics[0],0,3,translationVector[0]);

	SetValue(po_extrinsics[0],1,0,rotationMatrix[3]); 
	SetValue(po_extrinsics[0],1,1,rotationMatrix[4]); 
	SetValue(po_extrinsics[0],1,2,rotationMatrix[5]);
	SetValue(po_extrinsics[0],1,3,translationVector[1]);

	SetValue(po_extrinsics[0],2,0,rotationMatrix[6]); 
	SetValue(po_extrinsics[0],2,1,rotationMatrix[7]); 
	SetValue(po_extrinsics[0],2,2,rotationMatrix[8]);
	SetValue(po_extrinsics[0],2,3,translationVector[2]);

	SetValue(po_extrinsics[0],3,0,0); 
	SetValue(po_extrinsics[0],3,1,0); 
	SetValue(po_extrinsics[0],3,2,0);
	SetValue(po_extrinsics[0],3,3,1);
	


}
CvMat *POSIT::GetHomogeneousMatrix()
{
		return po_extrinsics[0];
}

double *POSIT::GetOpenGLMatrix()
{
	return po_posePOSIT;
}
void POSIT::GetImagePoints(IplImage *image)
{

	imageposit=image;
	omdata.Resize(po_nbpoints);
	global_points=po_nbpoints;
	MouseHandeler("Camera Image",1);
	ShowImage("Camera Image",image);
	WaitKey(0);
	
	po_imagePoints.clear();
	float cx=( (float*)(po_intrinsics[0]->data.ptr + po_intrinsics[0]->step*0) )[2];
	float cy= ( (float*)(po_intrinsics[0]->data.ptr + po_intrinsics[0]->step*1) )[2];
	for (int i = 0; i < po_nbpoints; i++) 
	{
		
			CvPoint2D32f point2D;
			
			point2D.x=(float)omdata.p[i].x- cx;
			point2D.y=cy-(float)omdata.p[i].y;
			po_imagePoints.push_back( point2D );
	}

}
void POSIT::GetObjetPoints(int nbpoints,std::vector<CvPoint3D32f > points)
{
		po_nbpoints=nbpoints;
		po_modelPoints=points;
		po_positObject = cvCreatePOSITObject( &po_modelPoints[0], (int)po_modelPoints.size() );

}

/** *********************class CameraCalibrator**********************************************************
*************************class CameraCalibrator**********************************************************
*************************class CameraCalibrator**********************************************************
*************************class CameraCalibrator**********************************************************
*************************class CameraCalibrator**********************************************************/

CameraCalibrator::CameraCalibrator()
{
}
CameraCalibrator::~CameraCalibrator()
{

}

int CameraCalibrator::CalibrateCamera(const std::vector<IplImage *> images,const CvSize& grid_dim,const int winx,int const winy,const float squaresize,CameraParameters &p)
{	
	//more than one image to extract intrinsic parameters in opencv bcoz opencv use homography to calculate parameters
	assert(images.size()>1);

	//image width and height
	CC_image_width=images[0]->width;
	CC_image_height=images[0]->height;
	
	//board dimension
	CC_board_size=grid_dim;
	
	//points in board
	CC_points_in_board=grid_dim.width*grid_dim.height;

	//boards nb=image nb (logique :D)
	CC_nb_boards=images.size();
	
	//window to draw corners 
	cvNamedWindow("Calibration");

	//initialize containers
	CvMat *image_points=cvCreateMat(CC_points_in_board*CC_nb_boards,2,CV_32FC1);
	CvMat *object_points=cvCreateMat(CC_points_in_board*CC_nb_boards,3,CV_32FC1);
	CvMat *point_counts=cvCreateMat(CC_nb_boards,1,CV_32SC1);
	CvMat *intrinsic_matrix=cvCreateMat(3,3,CV_64FC1);
	CvMat *distortion_coeffs=cvCreateMat(5,1,CV_64FC1);

	CvPoint2D32f* corners=new CvPoint2D32f[CC_points_in_board];

	int corner_count;
	int successes=0;
	int step,frame=0;
	
	int indexcam=0;
	

	//temp image  to convert input image to gray 
	IplImage *image;IplImage *gray_image;
	image=cvCreateImage(cvGetSize(images[0]),8,1);
	gray_image=cvCreateImage(cvGetSize(images[0]),8,1);
	//loop throught all images
	while(indexcam<CC_nb_boards)
	{
		//now verify to convert input images as a grayscale images
		if(images[indexcam]->nChannels!=1)
		{
				
			//cvConvertImage(images[indexcam],image,CV_BGR2GRAY);
			cvReleaseImage(&image);
			image=cvCloneImage(images[indexcam]);
		}

		
		//convert BGR -> gray
		cvCvtColor(images[indexcam],gray_image,CV_BGR2GRAY);

		//extract corners from current view
		cout<<"\nExtracting grid corners from image number: "<<indexcam<<"\n";
		int found=cvFindChessboardCorners(gray_image,CC_board_size,corners,&corner_count,CV_CALIB_CB_ADAPTIVE_THRESH  | CV_CALIB_CB_NORMALIZE_IMAGE );//CV_CALIB_CB_FILTER_QUADS CV_CALIB_CB_ADAPTIVE_THRESH	CV_CALIB_CB_NORMALIZE_IMAGE
		
		
		//re extract sub corners 
		cvFindCornerSubPix(gray_image,corners,corner_count,cvSize(winx,winy),cvSize(-1,-1),cvTermCriteria(CV_TERMCRIT_EPS+CV_TERMCRIT_ITER,30,0.1));

		//debug draw corners on chessboards
		cvDrawChessboardCorners(image,CC_board_size,corners,corner_count,found);
		cvShowImage("Calibration",image);
		int key=cvWaitKey(0);
		bool takeimage=true;
		switch(key)
		{
		case 'y':takeimage=true;break;
		case 'n':takeimage=false;break;
		default:takeimage=true;break;
		}
		
		//if its is a good corner extraction then store it
   		if(corner_count==CC_points_in_board && takeimage)
		{	
			
			

			//end \todo
			cout<<"grid corners extracted successfully from image number: "<<indexcam<<"\n";
			step=successes*CC_points_in_board;
			for(int i=step,j=0;j<CC_points_in_board;++i,++j)
			{	
	
				CV_MAT_ELEM(*image_points,float,i,0)=corners[j].x;
				CV_MAT_ELEM(*image_points,float,i,1)=corners[j].y;
				

				CV_MAT_ELEM(*object_points,float,i,0)=squaresize*(j/CC_board_size.width);
				CV_MAT_ELEM(*object_points,float,i,1)=squaresize*(j%CC_board_size.width);
				CV_MAT_ELEM(*object_points,float,i,2)=0.0f;

			}
			CV_MAT_ELEM(*point_counts,int,successes,0)=CC_points_in_board;
			successes++;
		}
		else cout<<"Failed to extract grid corners from image number: "<<indexcam<<". Skipping image...\n";
		
		//move to next image
		indexcam++;
	}
	
	if(successes<=1)
	{
	
	//free memory
	cvReleaseMat(&object_points);
	cvReleaseMat(&image_points);
	cvReleaseMat(&point_counts);
	cvReleaseImage(&gray_image);
	cvReleaseImage(&image);

	if(!successes) {
					std::cout<<"\n\nCould not extract corners from image sequence.check images again and verify the grid number that you supplied.\n\nFailed to calibrate...\n\n";
					cvDestroyAllWindows();
					return -2; 
					}
	if(successes==1) {	
						cout<<"\n\nExtracting corners from one image is not sufficient.\n\nFailed to calibrate...\n\n";
						cvDestroyAllWindows();
						return -1;
					}
	}
	//reallocate the objects_points and image points for successed images only
	CvMat *object_points2=cvCreateMat(successes*CC_points_in_board,3,CV_32FC1);
	CvMat *image_points2=cvCreateMat(successes*CC_points_in_board,2,CV_32FC1);	
	CvMat *point_counts2=cvCreateMat(successes,1,CV_32SC1);
	
	//copy only successed images
	for(int i=0;i<successes*CC_points_in_board;++i)
	{
		CV_MAT_ELEM(*image_points2,float,i,0)=CV_MAT_ELEM(*image_points,float,i,0);
		CV_MAT_ELEM(*image_points2,float,i,1)=CV_MAT_ELEM(*image_points,float,i,1);

		CV_MAT_ELEM(*object_points2,float,i,0)=CV_MAT_ELEM(*object_points,float,i,0);
		CV_MAT_ELEM(*object_points2,float,i,1)=CV_MAT_ELEM(*object_points,float,i,1);
		CV_MAT_ELEM(*object_points2,float,i,2)=CV_MAT_ELEM(*object_points,float,i,2);
	}
	
	for(int i=0;i<successes;++i)
	{
		CV_MAT_ELEM(*point_counts2,int,i,0)=CV_MAT_ELEM(*point_counts,int,i,0);
	}
	
	//free memory
	cvReleaseMat(&object_points);
	cvReleaseMat(&image_points);
	cvReleaseMat(&point_counts);

	//having all chessboard corners we have to initialize intrinsic matrix so that fx=fy=1
	CV_MAT_ELEM(*intrinsic_matrix,float,0,0)=1.0f;
	CV_MAT_ELEM(*intrinsic_matrix,float,1,1)=1.0f;

	//now everything is ready lets calibrate our camera.opencv will do the job 
	
	// comments Ricardo
	/*
	cout<<"\nDone Extraction...Calibrating camera...\n";
	cvCalibrateCamera2(object_points2,image_points2,point_counts2,cvGetSize(image),intrinsic_matrix,distortion_coeffs, NULL,NULL,CV_CALIB_FIX_K3 | CV_CALIB_FIX_PRINCIPAL_POINT  );
	cout<<"Calibration Done...\n";
	*/
	
	//print matrixs
	PrintMatrix(intrinsic_matrix,"Intrinsic Matrix");
	PrintMatrix(distortion_coeffs,"Distortion Matrix");

	//save parameters to xml file
	//cvSave("Intrinsics.xml",intrinsic_matrix);
	//cvSave("Distortion.xml",distortion_coeffs);

	
	

	//free memory
	cvReleaseMat(&object_points2);
	cvReleaseMat(&image_points2);
	cvReleaseMat(&point_counts2);
	cvReleaseImage(&gray_image);
	cvReleaseImage(&image);



	Stereo::TransformIntrinsicMatrixToCameraParameters(intrinsic_matrix,distortion_coeffs,p);
	//undistort images
	//build map
	/*
	std::vector<IplImage *> map=this->BuildUndistortionMap(p,images[0]->width,images[0]->height);
	
	int i=0;
	IplImage *undistot_imag;
	cvNamedWindow("Undistorted");
	cvNamedWindow("Distorted");
	while(i<images.size())
	{
		undistot_imag=UndistortImage(images[i],map);
		cvShowImage("Undistorted",undistot_imag);
		cvShowImage("Distorted",images[i]);
		cvWaitKey(20);
		i++;
	}
	*/
	//exit success
	cvDestroyAllWindows();
	return 0;
}
std::vector<IplImage *> CameraCalibrator::BuildUndistortionMap(const CameraParameters &c,const float imagewidth,const float imageheight) const
{
	

	std::vector<IplImage *> map;

	CvMat *intrinsic_matrix=cvCreateMat(3,3,CV_64F);
	CvMat *distortion_coeffs=cvCreateMat(5,1,CV_64F);
	
	Stereo::TransformCameraParametersToIntrinsicMatrix(intrinsic_matrix,distortion_coeffs,c);

	map.push_back(cvCreateImage(cvSize(imagewidth,imageheight),IPL_DEPTH_32F,1));
	map.push_back(cvCreateImage(cvSize(imagewidth,imageheight),IPL_DEPTH_32F,1));
	

	cvInitUndistortMap(intrinsic_matrix,distortion_coeffs,map[0],map[1]);
	return map;
	
}
IplImage* CameraCalibrator::UndistortImage(const IplImage *image,const std::vector<IplImage *> map) const
{	
	if(!map[0] || !map[1]) {cout<<"Chech mapx or mapy...\n";return NULL;}
	IplImage *t=cvCloneImage(image);
	cvRemap(image,t,map[0],map[1]);
	return t;
}
/** *********************class StereoCalibrator**********************************************************
*************************class StereoCalibrator**********************************************************
*************************class StereoCalibrator**********************************************************
*************************class StereoCalibrator**********************************************************
*************************class StereoCalibrator**********************************************************/
StereoCalibrator::StereoCalibrator()
{
}
StereoCalibrator::~StereoCalibrator()
{
}
int StereoCalibrator::CalibrateStereoCameras(
				const std::vector<IplImage *> left_images,
			const std::vector<IplImage *> right_images,
			CameraParameters &left_camera_parameters,
			CameraParameters &right_camera_parameters,
			CvMat* translationvector,
			CvMat*rotationmatrix,
			CvMat* rotationvec,
			const CvSize& grid_dim,
			CvMat* essential,
			CvMat*fundamental,
			const int winx,
			const int winy,
			const float squaresize,
			int fixintrinsic
			)
{


	CvMat *essential_matrix = cvCreateMat(3,3,CV_64F);
	CvMat *fundamental_matrix=cvCreateMat(3,3,CV_64F);
	CvMat *rotation=cvCreateMat(3,3,CV_64F);
	CvMat *translation=cvCreateMat(3,1,CV_64F);

	//more than one image to extract intrinsic parameters in opencv bcoz opencv use homography to calculate parameters
	assert(left_images.size()>1);
	assert(right_images.size()>1);
	assert(left_images.size()==right_images.size());

	//image width and height

	SC_image_width=left_images[0]->width;
	SC_image_height=left_images[0]->height;
	
	//board dimension
	SC_board_size=grid_dim;
	
	//points in board
	SC_points_in_board=grid_dim.width*grid_dim.height;

	//boards nb=image nb (logique :D)
	SC_nb_boards=left_images.size();
	
	//window to draw corners 
	cvNamedWindow("Left Calibration");
	cvNamedWindow("Right Calibration");

	//initialize containers
	CvMat *left_image_points=cvCreateMat(SC_points_in_board*SC_nb_boards,2,CV_32FC1);
	CvMat *right_image_points=cvCreateMat(SC_points_in_board*SC_nb_boards,2,CV_32FC1);
	CvMat *object_points=cvCreateMat(SC_points_in_board*SC_nb_boards,3,CV_32FC1);
	CvMat *point_counts=cvCreateMat(SC_nb_boards,1,CV_32SC1);
	
	CvMat *left_intrinsic_matrix=cvCreateMat(3,3,CV_64F);
	CvMat *left_distortion_coeffs=cvCreateMat(5,1,CV_64F);
	
	CvMat *right_intrinsic_matrix=cvCreateMat(3,3,CV_64F);
	CvMat *right_distortion_coeffs=cvCreateMat(5,1,CV_64F);

	//if(fixintrinsic==1)
//	{
		Stereo::TransformCameraParametersToIntrinsicMatrix(left_intrinsic_matrix,left_distortion_coeffs,left_camera_parameters);
		Stereo::TransformCameraParametersToIntrinsicMatrix(right_intrinsic_matrix,right_distortion_coeffs,right_camera_parameters);
//	}
//	else
	
//	{
	//	cvZero(left_intrinsic_matrix);
	//	cvZero(left_distortion_coeffs);

//	CV_MAT_ELEM(*left_intrinsic_matrix,float,0,0)=1.0f;
//	CV_MAT_ELEM(*left_intrinsic_matrix,float,1,1)=1.0f;


	//	cvZero(right_intrinsic_matrix);
	//	cvZero(right_distortion_coeffs);
	//CV_MAT_ELEM(*right_intrinsic_matrix,float,0,0)=1.0f;
//	CV_MAT_ELEM(*right_intrinsic_matrix,float,1,1)=1.0f;
//	}

	CvPoint2D32f* left_corners=new CvPoint2D32f[SC_points_in_board];
	CvPoint2D32f* right_corners=new CvPoint2D32f[SC_points_in_board];

	int left_corner_count;
	int right_corner_count;
	
	int successes=0;
	
	int step,frame=0;
	
	int indexcam=0;
	

	//temp image  to convert input image to gray 
	IplImage *left_image;IplImage *left_gray_image;
	IplImage *right_image;IplImage *right_gray_image;
	
	left_image=cvCreateImage(cvGetSize(left_images[0]),8,1);
	right_image=cvCreateImage(cvGetSize(right_images[0]),8,1);

	left_gray_image=cvCreateImage(cvGetSize(left_images[0]),8,1);
	right_gray_image=cvCreateImage(cvGetSize(right_images[0]),8,1);

	//loop throught all images
	while(indexcam<SC_nb_boards)
	{
		//now verify to convert input images as a grayscale images
		if(left_images[indexcam]->nChannels!=1 || right_images[indexcam]->nChannels!=1)
		{
				
			//cvConvertImage(images[indexcam],image,CV_BGR2GRAY);
			cvReleaseImage(&left_image);
			cvReleaseImage(&right_image);

			left_image=cvCloneImage(left_images[indexcam]);
			right_image=cvCloneImage(right_images[indexcam]);
		}

		
		
		//extract corners from current view
		cout<<"\nExtracting grid corners from left image number: "<<indexcam<<"\n";
		int left_found=cvFindChessboardCorners(left_images[indexcam],SC_board_size,left_corners,&left_corner_count,CV_CALIB_CB_ADAPTIVE_THRESH | CV_CALIB_CB_NORMALIZE_IMAGE );//CV_CALIB_CB_FILTER_QUADS CV_CALIB_CB_ADAPTIVE_THRESH	CV_CALIB_CB_NORMALIZE_IMAGE

		cout<<"\nExtracting grid corners from right image number: "<<indexcam<<"\n";
		int right_found=cvFindChessboardCorners(right_images[indexcam],SC_board_size,right_corners,&right_corner_count,CV_CALIB_CB_ADAPTIVE_THRESH | CV_CALIB_CB_NORMALIZE_IMAGE );//CV_CALIB_CB_FILTER_QUADS CV_CALIB_CB_ADAPTIVE_THRESH	CV_CALIB_CB_NORMALIZE_IMAGE
		
		//convert BGR -> gray
		cvCvtColor(left_images[indexcam],left_gray_image,CV_BGR2GRAY);
		cvCvtColor(right_images[indexcam],right_gray_image,CV_BGR2GRAY);
		
		//re extract sub corners 
		cvFindCornerSubPix(left_gray_image,left_corners,left_corner_count,cvSize(winx,winy),cvSize(-1,-1),cvTermCriteria(CV_TERMCRIT_EPS+CV_TERMCRIT_ITER,30,0.1));
		cvFindCornerSubPix(right_gray_image,right_corners,right_corner_count,cvSize(winx,winy),cvSize(-1,-1),cvTermCriteria(CV_TERMCRIT_EPS+CV_TERMCRIT_ITER,30,0.1));

		//debug draw corners on chessboards
		cvDrawChessboardCorners(left_image,SC_board_size,left_corners,left_corner_count,left_found);
		cvDrawChessboardCorners(right_image,SC_board_size,right_corners,right_corner_count,right_found);
		cvShowImage("Left Calibration",left_image);
		cvShowImage("Right Calibration",right_image);		
		int key=cvWaitKey(0);
		bool takeimages=true;
		switch(key)
		{
			case 'y':takeimages=true;break;
			case 'n':takeimages=false;break;
			default:takeimages=true;break;
		}
		
		//if its is a good corner extraction then store it
		if(left_corner_count==SC_points_in_board	&& right_corner_count==SC_points_in_board && takeimages)
		{	
			
			cout<<"grid corners extracted successfully from from image number: "<<indexcam<<" in left in right camera.\n";
			step=successes*SC_points_in_board;
			for(int i=step,j=0;j<SC_points_in_board;++i,++j)
			{	
	
				CV_MAT_ELEM(*left_image_points,float,i,0)=left_corners[j].x;
				CV_MAT_ELEM(*left_image_points,float,i,1)=left_corners[j].y;

				CV_MAT_ELEM(*right_image_points,float,i,0)=right_corners[j].x;
				CV_MAT_ELEM(*right_image_points,float,i,1)=right_corners[j].y;
				

				CV_MAT_ELEM(*object_points,float,i,0)=squaresize*(j/SC_board_size.width);
				CV_MAT_ELEM(*object_points,float,i,1)=squaresize*(j%SC_board_size.width);
				CV_MAT_ELEM(*object_points,float,i,2)=0.0f;

			}
			CV_MAT_ELEM(*point_counts,int,successes,0)=SC_points_in_board;
			successes++;
		}
		else cout<<"Failed to extract grid corners from image number: "<<indexcam<<". Skipping image...\n";
		
		//move to next image
		indexcam++;
	}
	
	if(successes<=1)
	{
	
	//free memory
	cvReleaseMat(&object_points);
	cvReleaseMat(&left_image_points);
	cvReleaseMat(&right_image_points);
	cvReleaseMat(&point_counts);
	cvReleaseImage(&left_gray_image);
	cvReleaseImage(&right_gray_image);
	cvReleaseImage(&left_image);
	cvReleaseImage(&right_image);

	if(!successes) {
					std::cout<<"\n\nCould not extract corners from image sequence.check images again and verify the grid number that you supplied.\n\nFailed to calibrate...\n\n";
					cvDestroyAllWindows();
					return -2; 
					}
	if(successes==1) {	
						cout<<"\n\nExtracting corners from one image is not sufficient.\n\nFailed to calibrate...\n\n";
						cvDestroyAllWindows();
						return -1;
					}
	}
	//reallocate the objects_points and image points for successed images only
	CvMat *object_points2=cvCreateMat(successes*SC_points_in_board,3,CV_32FC1);
	CvMat *left_image_points2=cvCreateMat(successes*SC_points_in_board,2,CV_32FC1);	
	CvMat *right_image_points2=cvCreateMat(successes*SC_points_in_board,2,CV_32FC1);	
	CvMat *point_counts2=cvCreateMat(successes,1,CV_32SC1);
	
	//copy only successed images
	for(int i=0;i<successes*SC_points_in_board;++i)
	{
		CV_MAT_ELEM(*left_image_points2,float,i,0)=CV_MAT_ELEM(*left_image_points,float,i,0);
		CV_MAT_ELEM(*left_image_points2,float,i,1)=CV_MAT_ELEM(*left_image_points,float,i,1);

		CV_MAT_ELEM(*right_image_points2,float,i,0)=CV_MAT_ELEM(*right_image_points,float,i,0);
		CV_MAT_ELEM(*right_image_points2,float,i,1)=CV_MAT_ELEM(*right_image_points,float,i,1);

		CV_MAT_ELEM(*object_points2,float,i,0)=CV_MAT_ELEM(*object_points,float,i,0);
		CV_MAT_ELEM(*object_points2,float,i,1)=CV_MAT_ELEM(*object_points,float,i,1);
		CV_MAT_ELEM(*object_points2,float,i,2)=CV_MAT_ELEM(*object_points,float,i,2);
	}
	
	for(int i=0;i<successes;++i)
	{
		CV_MAT_ELEM(*point_counts2,int,i,0)=CV_MAT_ELEM(*point_counts,int,i,0);
	}
	
	//free memory
	cvReleaseMat(&object_points);
	cvReleaseMat(&left_image_points);
	cvReleaseMat(&right_image_points);
	cvReleaseMat(&point_counts);

	//having all chessboard corners we have to initialize intrinsic matrix so that fx=fy=1
	//CV_MAT_ELEM(*intrinsic_matrix,float,0,0)=1.0f;
	//CV_MAT_ELEM(*intrinsic_matrix,float,1,1)=1.0f;

	//now everything is ready lets calibrate our cameras.opencv will do the job 
	
	cout<<"\nDone Extraction...Calibrating stereo cameras...\n";
	//cvCalibrateCamera2(object_points2,image_points2,point_counts2,cvGetSize(image),intrinsic_matrix,distortion_coeffs);
	vector<int> npoints;
	npoints.resize(successes,SC_board_size.width*SC_board_size.height);
	CvMat count=cvMat(1,successes,CV_32S,&npoints[0]);

	if(!fixintrinsic)
	{
		PrintMatrix(left_intrinsic_matrix,"left Intrinsic Matrix Before Calibration");
		PrintMatrix(left_distortion_coeffs,"left Distortion Matrix Before Calibration");

		PrintMatrix(right_intrinsic_matrix,"right Intrinsic Matrix Before Calibration");
 		PrintMatrix(right_distortion_coeffs,"right Distortion Matrix Before Calibration");
	}
	
		//comments Ricardo
	/*unsigned int  flags=CV_CALIB_FIX_K3;
	flags |=CV_CALIB_ZERO_TANGENT_DIST;
	if(!fixintrinsic)
		 flags|= CV_CALIB_USE_INTRINSIC_GUESS;

	else flags|=CV_CALIB_FIX_INTRINSIC;*/

	//comments Ricardo
	/*cvStereoCalibrate(
		object_points2,
		left_image_points2,
		right_image_points2,
		point_counts2,
		left_intrinsic_matrix,
		left_distortion_coeffs,
		right_intrinsic_matrix,
		right_distortion_coeffs,
		cvGetSize(left_image),
		rotation,
		translation,
		essential_matrix,
		fundamental_matrix,
		cvTermCriteria(CV_TERMCRIT_ITER+
		CV_TERMCRIT_EPS, 100, 1e-5),
		flags
		);
	cout<<"Calibration Done...\n";
	*/


	//print matrixs
	PrintMatrix(left_intrinsic_matrix,"left Intrinsic Matrix After stereo-optimization");
	PrintMatrix(left_distortion_coeffs,"left Distortion Matrix After stereo-optimization");

	PrintMatrix(right_intrinsic_matrix,"right Intrinsic Matrix After stereo-optimization");
	PrintMatrix(right_distortion_coeffs,"right Distortion Matrix After stereo-optimization");

	
	
	CvMat *rotationvector=cvCreateMat(3,1,CV_64F);
	cvRodrigues2(rotation,rotationvector);
	
	PrintMatrix(rotation,"Rotation matrix ");
	PrintMatrix(rotationvector,"Rotation vector ");
	PrintMatrix(translation,"Translation vector");

	PrintMatrix(essential_matrix,"Essential matrix");
	PrintMatrix(fundamental_matrix,"Fundamental matrix");

	//save parameters to xml file
	//cvSave("Intrinsics.xml",intrinsic_matrix);
	//cvSave("Distortion.xml",distortion_coeffs);

	
	//copy to output
	
	//translation vector
	CV_MAT_ELEM(*translationvector,double,0,0)=CV_MAT_ELEM(*translation,double,0,0);
	CV_MAT_ELEM(*translationvector,double,1,0)=CV_MAT_ELEM(*translation,double,1,0);
	CV_MAT_ELEM(*translationvector,double,2,0)=CV_MAT_ELEM(*translation,double,2,0);
	
	//rotation vector
	CV_MAT_ELEM(*rotationvec,double,0,0)=CV_MAT_ELEM(*rotationvector,double,0,0);
	CV_MAT_ELEM(*rotationvec,double,1,0)=CV_MAT_ELEM(*rotationvector,double,1,0);
	CV_MAT_ELEM(*rotationvec,double,2,0)=CV_MAT_ELEM(*rotationvector,double,2,0);

	//rotation matrix
	CV_MAT_ELEM(*rotationmatrix,double,0,0)=CV_MAT_ELEM(*rotation,double,0,0);
	CV_MAT_ELEM(*rotationmatrix,double,0,1)=CV_MAT_ELEM(*rotation,double,0,1);
	CV_MAT_ELEM(*rotationmatrix,double,0,2)=CV_MAT_ELEM(*rotation,double,0,2);

	CV_MAT_ELEM(*rotationmatrix,double,1,0)=CV_MAT_ELEM(*rotation,double,1,0);
	CV_MAT_ELEM(*rotationmatrix,double,1,1)=CV_MAT_ELEM(*rotation,double,1,1);
	CV_MAT_ELEM(*rotationmatrix,double,1,2)=CV_MAT_ELEM(*rotation,double,1,2);

	CV_MAT_ELEM(*rotationmatrix,double,2,0)=CV_MAT_ELEM(*rotation,double,2,0);
	CV_MAT_ELEM(*rotationmatrix,double,2,1)=CV_MAT_ELEM(*rotation,double,2,1);
	CV_MAT_ELEM(*rotationmatrix,double,2,2)=CV_MAT_ELEM(*rotation,double,2,2);

	//essential matrix
	CV_MAT_ELEM(*essential,double,0,0)=CV_MAT_ELEM(*essential_matrix,double,0,0);
	CV_MAT_ELEM(*essential,double,0,1)=CV_MAT_ELEM(*essential_matrix,double,0,1);
	CV_MAT_ELEM(*essential,double,0,2)=CV_MAT_ELEM(*essential_matrix,double,0,2);

	CV_MAT_ELEM(*essential,double,1,0)=CV_MAT_ELEM(*essential_matrix,double,1,0);
	CV_MAT_ELEM(*essential,double,1,1)=CV_MAT_ELEM(*essential_matrix,double,1,1);
	CV_MAT_ELEM(*essential,double,1,2)=CV_MAT_ELEM(*essential_matrix,double,1,2);

	CV_MAT_ELEM(*essential,double,2,0)=CV_MAT_ELEM(*essential_matrix,double,2,0);
	CV_MAT_ELEM(*essential,double,2,1)=CV_MAT_ELEM(*essential_matrix,double,2,1);
	CV_MAT_ELEM(*essential,double,2,2)=CV_MAT_ELEM(*essential_matrix,double,2,2);

	//fundamental matrix
	CV_MAT_ELEM(*fundamental,double,0,0)=CV_MAT_ELEM(*fundamental_matrix,double,0,0);
	CV_MAT_ELEM(*fundamental,double,0,1)=CV_MAT_ELEM(*fundamental_matrix,double,0,1);
	CV_MAT_ELEM(*fundamental,double,0,2)=CV_MAT_ELEM(*fundamental_matrix,double,0,2);

	CV_MAT_ELEM(*fundamental,double,1,0)=CV_MAT_ELEM(*fundamental_matrix,double,1,0);
	CV_MAT_ELEM(*fundamental,double,1,1)=CV_MAT_ELEM(*fundamental_matrix,double,1,1);
	CV_MAT_ELEM(*fundamental,double,1,2)=CV_MAT_ELEM(*fundamental_matrix,double,1,2);

	CV_MAT_ELEM(*fundamental,double,2,0)=CV_MAT_ELEM(*fundamental_matrix,double,2,0);
	CV_MAT_ELEM(*fundamental,double,2,1)=CV_MAT_ELEM(*fundamental_matrix,double,2,1);
	CV_MAT_ELEM(*fundamental,double,2,2)=CV_MAT_ELEM(*fundamental_matrix,double,2,2);


	
	
	

	//free memory

	cvReleaseMat(&translation);
	cvReleaseMat(&rotationvector);
	cvReleaseMat(&rotation);
	cvReleaseMat(&essential_matrix);
	cvReleaseMat(&fundamental_matrix);

	cvReleaseMat(&object_points2);
	cvReleaseMat(&left_image_points2);
	cvReleaseMat(&right_image_points2);

	cvReleaseMat(&point_counts2);
	cvReleaseImage(&left_gray_image);
	cvReleaseImage(&right_gray_image);
	cvReleaseImage(&left_image);
	cvReleaseImage(&right_image);


return 0;


}
};